import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
from launch.actions import IncludeLaunchDescription, GroupAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    创建一个实车tag检测的最小系统，方便进行定位系统准确度评价
    """

    package_path = get_package_share_directory("robot_define")
    urdf_file = os.path.join(package_path, "urdf", "robot_real.urdf")
    with open(urdf_file, "r") as infp:
        urdf_content = infp.read()

    state_publisher_node1 = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher_node",
        output="screen",
        parameters=[{"robot_description": urdf_content}, {"frame_prefix": "robot1/"}],
        namespace="robot1",
    )
    state_publisher_node2 = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher_node",
        output="screen",
        parameters=[{"robot_description": urdf_content}, {"frame_prefix": "robot2/"}],
        namespace="robot2",
    )

    camera = Node(
        package="april_tag_finder",
        executable="camera_mono",
        name="camera_publisher_node",
        output="screen",
        parameters=[
            {
                "device": "/dev/video0",
                "width": 1280,
                "height": 720,
                "fps": 30,
                "flip_vertical": True,
                "flip_horizontal": True,
            }
        ],
        namespace="robot1",
    )

    tagFinder = Node(
        package="april_tag_finder",
        executable="finder_auto",
        name="tag_finder",
        remappings=[
            ("camera_image", "camera_image"),
            ("camera_info", "camera_info"),
        ],
        parameters=[
            {"debug": True},
            {"tag_size": 0.04},
            {"tag_family": "tag36h11"},
        ],
        output="screen",
        namespace="robot1",
    )

    tag2chassis = Node(
        package="april_tag_finder",
        executable="tag2chassis",
        name="tag2chassis",
        output="screen",
        namespace="robot1",
        parameters=[
            {"frame_prefix": "robot1"},
            {"target_prefix": "robot2"},
        ],
    )

    static_tf_node_tag2robot2 = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="initial_pose_publisher_node",
        output="screen",
        arguments=["0", "0", "0", "0", "0", "0", "/robot1/tag_chassis", "/robot2/chassis"],
    )
    static_tf_node_map2chassis = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="initial_pose_publisher_node",
        output="screen",
        arguments=["0", "0", "0", "0", "0", "0", "map", "/robot1/chassis"],
    )

    return LaunchDescription(
        [
            state_publisher_node1,
            state_publisher_node2,
            camera,
            tagFinder,
            tag2chassis,
            static_tf_node_tag2robot2,
            static_tf_node_map2chassis,
        ]
    )
